home *** CD-ROM | disk | FTP | other *** search
/ Power Programmierung 2 / Power-Programmierung CD 2 (Tewi)(1994).iso / gnu / gnulib / sipp / libsipp / geometri.c < prev    next >
Encoding:
C/C++ Source or Header  |  1992-05-03  |  10.4 KB  |  404 lines
  1. /**
  2.  ** sipp - SImple Polygon Processor
  3.  **
  4.  **  A general 3d graphic package
  5.  **
  6.  **  Copyright Equivalent Software HB  1992
  7.  **
  8.  ** This program is free software; you can redistribute it and/or modify
  9.  ** it under the terms of the GNU General Public License as published by
  10.  ** the Free Software Foundation; either version 1, or any later version.
  11.  ** This program is distributed in the hope that it will be useful,
  12.  ** but WITHOUT ANY WARRANTY; without even the implied warranty of
  13.  ** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  14.  ** GNU General Public License for more details.
  15.  ** You can receive a copy of the GNU General Public License from the
  16.  ** Free Software Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  17.  **/
  18.  
  19. /**
  20.  ** geometric.c - Matrixes, transformations and coordinates.
  21.  **/
  22.  
  23. #include <stdio.h>
  24. #include <math.h>
  25.  
  26. #include <sipp.h>
  27. #include <smalloc.h>
  28. #include <geometric.h>
  29.  
  30.  
  31. /* =================================================================== */
  32.  /* */
  33.  
  34.  
  35. Transf_mat   ident_matrix = {{        /* Unit tranfs. matrix */
  36.     { 1.0, 0.0, 0.0 },
  37.     { 0.0, 1.0, 0.0 },
  38.     { 0.0, 0.0, 1.0 },
  39.     { 0.0, 0.0, 0.0 }
  40. }};
  41.  
  42.  
  43. /* =================================================================== */
  44.  
  45. /*
  46.  * Allocate a new matrix, and if INITMAT != NULL copy the contents
  47.  * of INITMAT to the new matrix,  otherwise copy the identity matrix
  48.  * to the new matrix.
  49.  */
  50.  
  51. Transf_mat *
  52. transf_mat_create(initmat)
  53.     Transf_mat  * initmat;
  54. {
  55.     Transf_mat  * mat;
  56.  
  57.     mat = (Transf_mat *) smalloc(sizeof(Transf_mat));
  58.     if (initmat != NULL)
  59.     MatCopy(mat, initmat);
  60.     else
  61.     MatCopy(mat, &ident_matrix);
  62.  
  63.     return mat;
  64. }
  65.  
  66.  
  67. void
  68. transf_mat_destruct(mat)
  69.     Transf_mat  * mat;
  70. {
  71.     sfree(mat);
  72. }
  73.  
  74.  
  75. /* =================================================================== */
  76. /*          Transformation routines (see also geometric.h)             */
  77.  
  78.  
  79. /*
  80.  * Normalize a vector.
  81.  */
  82. void
  83. vecnorm(vec)
  84.     Vector  *vec;
  85. {
  86.     double   len;
  87.  
  88.     len =  VecLen(*vec);
  89.     VecScalMul(*vec, 1.0 / len, *vec);
  90. }
  91.  
  92.  
  93.  
  94. /*
  95.  * Set MAT to the transformation matrix that represents the 
  96.  * concatenation between the previous transformation in MAT
  97.  * and a translation along the vector described by DX, DY and DZ.
  98.  *
  99.  * [a  b  c  0]   [ 1  0  0  0]     [ a     b     c    0]
  100.  * [d  e  f  0]   [ 0  1  0  0]     [ d     e     f    0]
  101.  * [g  h  i  0] * [ 0  0  1  0]  =  [ g     h     i    0]
  102.  * [j  k  l  1]   [Tx Ty Tz  1]     [j+Tx  k+Ty  l+Tz  1]
  103.  */
  104.  
  105. void
  106. mat_translate(mat,  dx,  dy,  dz)
  107.     Transf_mat  * mat;
  108.     double        dx;
  109.     double        dy; 
  110.     double        dz;
  111. {
  112.     mat->mat[3][0] += dx;
  113.     mat->mat[3][1] += dy;
  114.     mat->mat[3][2] += dz;
  115. }
  116.  
  117.  
  118.  
  119. /*
  120.  * Set MAT to the transformation matrix that represents the 
  121.  * concatenation between the previous transformation in MAT
  122.  * and a rotation with the angle ANG around the X axis.
  123.  *
  124.  * [a  b  c  0]   [1   0   0  0]     [a  b*Ca-c*Sa  b*Sa+c*Ca  0]
  125.  * [d  e  f  0]   [0  Ca  Sa  0]     [d  e*Ca-f*Sa  e*Sa+f*Ca  0]
  126.  * [g  h  i  0] * [0 -Sa  Ca  0]  =  [g  h*Ca-i*Sa  h*Sa+i*Ca  0]
  127.  * [j  k  l  1]   [0   0   0  1]     [j  k*Ca-l*Sa  k*Se+l*Ca  1]
  128.  */
  129.  
  130. void
  131. mat_rotate_x(mat, ang)
  132.     Transf_mat  * mat;
  133.     double        ang;
  134. {
  135.     double   cosang;
  136.     double   sinang;
  137.     double   tmp;
  138.     int      i;
  139.     
  140.     cosang = cos(ang);
  141.     sinang = sin(ang);
  142.     if (fabs(cosang) < 1.0e-15) {
  143.         cosang = 0.0;
  144.     }
  145.     if (fabs(sinang) < 1.0e-15) {
  146.         sinang = 0.0;
  147.     }
  148.     for (i = 0; i < 4; ++i) {
  149.     tmp = mat->mat[i][1];
  150.     mat->mat[i][1] = mat->mat[i][1] * cosang
  151.                    - mat->mat[i][2] * sinang;
  152.     mat->mat[i][2] = tmp * sinang + mat->mat[i][2] * cosang;
  153.     }
  154. }
  155.  
  156.  
  157.  
  158. /*
  159.  * Set MAT to the transformation matrix that represents the 
  160.  * concatenation between the previous transformation in MAT
  161.  * and a rotation with the angle ANG around the Y axis.
  162.  *
  163.  * [a  b  c  0]   [Ca   0 -Sa  0]     [a*Ca+c*Sa  b  -a*Sa+c*Ca  0]
  164.  * [d  e  f  0] * [ 0   1   0  0]  =  [d*Ca+f*Sa  e  -d*Sa+f*Ca  0]
  165.  * [g  h  i  0]   [Sa   0  Ca  0]     [g*Ca+i*Sa  h  -g*Sa+i*Ca  0]
  166.  * [j  k  l  1]   [ 0   0   0  1]     [j*Ca+l*Sa  k  -j*Sa+l*Ca  1]
  167.  */
  168.  
  169. void
  170. mat_rotate_y(mat, ang)
  171.     Transf_mat  * mat;
  172.     double        ang;
  173. {
  174.     double   cosang;
  175.     double   sinang;
  176.     double   tmp;
  177.     int      i;
  178.     
  179.     cosang = cos(ang);
  180.     sinang = sin(ang);
  181.     if (fabs(cosang) < 1.0e-15) {
  182.         cosang = 0.0;
  183.     }
  184.     if (fabs(sinang) < 1.0e-15) {
  185.         sinang = 0.0;
  186.     }
  187.     for (i = 0; i < 4; ++i) {
  188.     tmp = mat->mat[i][0];
  189.     mat->mat[i][0] = mat->mat[i][0] * cosang
  190.                    + mat->mat[i][2] * sinang;
  191.     mat->mat[i][2] = -tmp * sinang + mat->mat[i][2] * cosang;
  192.     }
  193. }
  194.  
  195.  
  196.  
  197. /*
  198.  * Set MAT to the transformation matrix that represents the 
  199.  * concatenation between the previous transformation in MAT
  200.  * and a rotation with the angle ANG around the Z axis.
  201.  *
  202.  * [a  b  c  0]   [ Ca  Sa   0  0]     [a*Ca-b*Sa  a*Sa+b*Ca  c  0]
  203.  * [d  e  f  0]   [-Sa  Ca   0  0]     [d*Ca-e*Sa  d*Sa+e*Ca  f  0]
  204.  * [g  h  i  0] * [  0   0   1  0]  =  [g*Ca-h*Sa  g*Sa+h*Ca  i  0]
  205.  * [j  k  l  1]   [  0   0   0  1]     [j*Ca-k*Sa  j*Sa+k*Ca  l  0]
  206.  */
  207.  
  208. void
  209. mat_rotate_z(mat, ang)
  210.     Transf_mat  * mat;
  211.     double        ang;
  212. {
  213.     double   cosang;
  214.     double   sinang;
  215.     double   tmp;
  216.     int      i;
  217.     
  218.     cosang = cos(ang);
  219.     sinang = sin(ang);
  220.     if (fabs(cosang) < 1.0e-15) {
  221.         cosang = 0.0;
  222.     }
  223.     if (fabs(sinang) < 1.0e-15) {
  224.         sinang = 0.0;
  225.     }
  226.     for (i = 0; i < 4; ++i) {
  227.     tmp = mat->mat[i][0];
  228.     mat->mat[i][0] = mat->mat[i][0] * cosang
  229.                    - mat->mat[i][1] * sinang;
  230.     mat->mat[i][1] = tmp * sinang + mat->mat[i][1] * cosang;
  231.     }
  232. }
  233.  
  234.  
  235.  
  236. /*
  237.  * Set MAT to the transformation matrix that represents the
  238.  * concatenation between the previous transformation in MAT
  239.  * and a rotation with the angle ANG around the line represented
  240.  * by the point POINT and the vector VECTOR.
  241.  */
  242.  
  243. void
  244. mat_rotate(mat, point, vector, ang)
  245.     Transf_mat  * mat;
  246.     Vector      * point;
  247.     Vector      * vector;
  248.     double        ang;
  249. {
  250.     double   ang2;
  251.     double   ang3;
  252.  
  253.     ang2 = atan2(vector->y, vector->x);
  254.     ang3 = atan2(hypot(vector->x, vector->y), vector->z);
  255.     mat_translate(mat, -point->x, -point->y, -point->z);
  256.     mat_rotate_z(mat, -ang2);
  257.     mat_rotate_y(mat, -ang3);
  258.     mat_rotate_z(mat, ang);
  259.     mat_rotate_y(mat, ang3);
  260.     mat_rotate_z(mat, ang2);
  261.     mat_translate(mat, point->x, point->y, point->z);
  262. }
  263.  
  264.  
  265.  
  266. /*
  267.  * Set MAT to the transformation matrix that represents the 
  268.  * concatenation between the previous transformation in MAT
  269.  * and a scaling with the scaling factors XSCALE, YSCALE and ZSCALE
  270.  *
  271.  * [a  b  c  0]   [Sx  0  0  0]     [a*Sx  b*Sy  c*Sz  0]
  272.  * [d  e  f  0]   [ 0 Sy  0  0]     [d*Sx  e*Sy  f*Sz  0]
  273.  * [g  h  i  0] * [ 0  0 Sz  0]  =  [g*Sx  h*Sy  i*Sz  0]
  274.  * [j  k  l  1]   [ 0  0  0  1]     [j*Sx  k*Sy  l*Sz  1]
  275.  */
  276.  
  277. void
  278. mat_scale(mat, xscale, yscale, zscale)
  279.     Transf_mat  * mat;
  280.     double        xscale;
  281.     double        yscale;
  282.     double        zscale;
  283. {
  284.     int   i;
  285.  
  286.     for (i = 0; i < 4; ++i) {
  287.     mat->mat[i][0] *= xscale;
  288.     mat->mat[i][1] *= yscale;
  289.     mat->mat[i][2] *= zscale;
  290.     }
  291. }
  292.  
  293.  
  294.  
  295. /*
  296.  * Set MAT to the transformation matrix that represents the
  297.  * concatenation between the previous transformation in MAT
  298.  * and a mirroring in the plane defined by the point POINT
  299.  * and the normal vector NORM.
  300.  */
  301.  
  302. void
  303. mat_mirror_plane(mat, point, norm)
  304.     Transf_mat  * mat;
  305.     Vector      * point;
  306.     Vector      * norm;
  307. {
  308.     Transf_mat   tmp;
  309.     double   factor;
  310.  
  311.     /* The first thing we do is to make a transformation matrix */
  312.     /* for mirroring through a plane with the same normal vector */
  313.     /* as our, but through the origin instead. */
  314.     factor = 2.0 / (norm->x * norm->x + norm->y * norm->y 
  315.             + norm->z * norm->z);
  316.     
  317.     /* The diagonal elements. */
  318.     tmp.mat[0][0] = 1 - factor * norm->x * norm->x;
  319.     tmp.mat[1][1] = 1 - factor * norm->y * norm->y;
  320.     tmp.mat[2][2] = 1 - factor * norm->z * norm->z;
  321.     
  322.     /* The rest of the matrix */
  323.     tmp.mat[1][0] = tmp.mat[0][1] = -factor * norm->x * norm->y;
  324.     tmp.mat[2][0] = tmp.mat[0][2] = -factor * norm->x * norm->z;
  325.     tmp.mat[2][1] = tmp.mat[1][2] = -factor * norm->y * norm->z;
  326.     tmp.mat[3][0] = tmp.mat[3][1] = tmp.mat[3][2] = 0.0;
  327.  
  328.     /* Do the actual transformation. This is done in 3 steps: */
  329.     /* 1) Translate the plane so that it goes through the origin. */
  330.     /* 2) Do the actual mirroring. */
  331.     /* 3) Translate it all back to the starting position. */
  332.     mat_translate(mat, -point->x, -point->y, -point->z);
  333.     mat_mul(mat, mat, &tmp);
  334.     mat_translate(mat, point->x, point->y, point->z);
  335. }
  336.  
  337.  
  338.  
  339. /*
  340.  * Multiply the Matrix A with the Matrix B, and store the result
  341.  * into the Matrix RES. It is possible for RES to point to the 
  342.  * same Matrix as either A or B since the result is stored into
  343.  * a temporary during computation.
  344.  *
  345.  * [a b c 0]  [A B C 0]     [aA+bD+cG    aB+bE+cH    aC+bF+cI    0]
  346.  * [d e f 0]  [D E F 0]     [dA+eD+fG    dB+eE+fH    dC+eF+fI    0]
  347.  * [g h i 0]  [G H I 0]  =  [gA+hD+iG    gB+hE+iH    gC+hF+iI    0]
  348.  * [j k l 1]  [J K L 1]     [jA+kD+lG+J  jB+kE+lH+K  jC+kF+lI+L  1]
  349.  */
  350.  
  351. void
  352. mat_mul(res, a, b)
  353.     Transf_mat  * res;
  354.     Transf_mat  * a;
  355.     Transf_mat  * b;
  356. {
  357.     Transf_mat   tmp;
  358.     int      i;
  359.  
  360.     for (i = 0; i < 4; ++i) {
  361.     tmp.mat[i][0] = a->mat[i][0] * b->mat[0][0]
  362.                   + a->mat[i][1] * b->mat[1][0] 
  363.                   + a->mat[i][2] * b->mat[2][0];
  364.     tmp.mat[i][1] = a->mat[i][0] * b->mat[0][1]
  365.                     + a->mat[i][1] * b->mat[1][1] 
  366.                   + a->mat[i][2] * b->mat[2][1];
  367.     tmp.mat[i][2] = a->mat[i][0] * b->mat[0][2]
  368.                    + a->mat[i][1] * b->mat[1][2]
  369.                   + a->mat[i][2] * b->mat[2][2];
  370.     }
  371.  
  372.     tmp.mat[3][0] += b->mat[3][0];
  373.     tmp.mat[3][1] += b->mat[3][1];
  374.     tmp.mat[3][2] += b->mat[3][2];
  375.  
  376.     MatCopy(res, &tmp);
  377. }
  378.  
  379.  
  380.  
  381. /*
  382.  * Transform the Point3d VEC with the transformation matrix MAT, and
  383.  * put the result into the vector *RES.
  384.  *
  385.  *               [a  b  c  0]
  386.  *               [d  e  f  0]
  387.  * [x  y  z  1]  [g  h  i  0]  =  [ax+dy+gz+j  bx+ey+hz+k  cx+fy+iz+l  1]
  388.  *               [j  k  l  1]
  389.  */
  390.  
  391. void
  392. point_transform(res, vec, mat)
  393.     Vector      * res;
  394.     Vector      * vec;
  395.     Transf_mat  * mat;
  396. {
  397.     res->x = mat->mat[0][0] * vec->x + mat->mat[1][0] * vec->y 
  398.         + mat->mat[2][0] * vec->z + mat->mat[3][0];
  399.     res->y = mat->mat[0][1] * vec->x + mat->mat[1][1] * vec->y 
  400.         + mat->mat[2][1] * vec->z + mat->mat[3][1];
  401.     res->z = mat->mat[0][2] * vec->x + mat->mat[1][2] * vec->y 
  402.         + mat->mat[2][2] * vec->z + mat->mat[3][2];
  403. }
  404.